/**
* Copyright (c) By zengqh.
*
* This program is just for fun or demo, in the hope that it  
* will be useful, you can redistribute it and/or modify freely.
*
* Time: 2013/02/18
* File: enn_matrix3x4.h
**/

#pragma once

#include "enn_matrix4.h"

namespace enn
{
/// 3x4 matrix for scene node transform calculations.
class Matrix3x4
{
public:
	/// Construct undefined.
	Matrix3x4()
	{
	}

	/// Copy-construct from another matrix.
	Matrix3x4(const Matrix3x4& matrix) :
	m00_(matrix.m00_),
		m01_(matrix.m01_),
		m02_(matrix.m02_),
		m03_(matrix.m03_),
		m10_(matrix.m10_),
		m11_(matrix.m11_),
		m12_(matrix.m12_),
		m13_(matrix.m13_),
		m20_(matrix.m20_),
		m21_(matrix.m21_),
		m22_(matrix.m22_),
		m23_(matrix.m23_)
	{
	}

	/// Copy-construct from a 3x3 matrix and set the extra elements to identity.
	Matrix3x4(const Matrix3& matrix) :
	m00_(matrix.m00_),
		m01_(matrix.m01_),
		m02_(matrix.m02_),
		m03_(0.0f),
		m10_(matrix.m10_),
		m11_(matrix.m11_),
		m12_(matrix.m12_),
		m13_(0.0f),
		m20_(matrix.m20_),
		m21_(matrix.m21_),
		m22_(matrix.m22_),
		m23_(0.0f)
	{
	}

	// Construct from values.
	Matrix3x4(float v00, float v01, float v02, float v03,
		float v10, float v11, float v12, float v13,
		float v20, float v21, float v22, float v23) :
	m00_(v00),
		m01_(v01),
		m02_(v02),
		m03_(v03),
		m10_(v10),
		m11_(v11),
		m12_(v12),
		m13_(v13),
		m20_(v20),
		m21_(v21),
		m22_(v22),
		m23_(v23)
	{
	}

	/// Construct from a float array.
	Matrix3x4(const float* data) :
	m00_(data[0]),
		m01_(data[1]),
		m02_(data[2]),
		m03_(data[3]),
		m10_(data[4]),
		m11_(data[5]),
		m12_(data[6]),
		m13_(data[7]),
		m20_(data[8]),
		m21_(data[9]),
		m22_(data[10]),
		m23_(data[11])
	{
	}

	/// Construct from translation, rotation and uniform scale.
	Matrix3x4(const vec3f& translation, const Quaternion& rotation, float scale);
	/// Construct from translation, rotation and nonuniform scale.
	Matrix3x4(const vec3f& translation, const Quaternion& rotation, const vec3f& scale);

	/// Assign from another matrix.
	Matrix3x4& operator = (const Matrix3x4& rhs)
	{
		m00_ = rhs.m00_;
		m01_ = rhs.m01_;
		m02_ = rhs.m02_;
		m03_ = rhs.m03_;
		m10_ = rhs.m10_;
		m11_ = rhs.m11_;
		m12_ = rhs.m12_;
		m13_ = rhs.m13_;
		m20_ = rhs.m20_;
		m21_ = rhs.m21_;
		m22_ = rhs.m22_;
		m23_ = rhs.m23_;
		return *this;
	}

	/// Assign from a 3x3 matrix and set the extra elements to identity.
	Matrix3x4& operator = (const Matrix3& rhs)
	{
		m00_ = rhs.m00_;
		m01_ = rhs.m01_;
		m02_ = rhs.m02_;
		m03_ = 0.0;
		m10_ = rhs.m10_;
		m11_ = rhs.m11_;
		m12_ = rhs.m12_;
		m13_ = 0.0;
		m20_ = rhs.m20_;
		m21_ = rhs.m21_;
		m22_ = rhs.m22_;
		m23_ = 0.0;
		return *this;
	}

	/// Multiply a vec3f which is assumed to represent position.
	vec3f operator * (const vec3f& rhs) const
	{
		return vec3f(
			(m00_ * rhs.x_ + m01_ * rhs.y_ + m02_ * rhs.z_ + m03_),
			(m10_ * rhs.x_ + m11_ * rhs.y_ + m12_ * rhs.z_ + m13_),
			(m20_ * rhs.x_ + m21_ * rhs.y_ + m22_ * rhs.z_ + m23_)
			);
	}

	/// Multiply a vec4f.
	vec3f operator * (const vec4f& rhs) const
	{
		return vec3f(
			(m00_ * rhs.x_ + m01_ * rhs.y_ + m02_ * rhs.z_ + m03_ * rhs.w_),
			(m10_ * rhs.x_ + m11_ * rhs.y_ + m12_ * rhs.z_ + m13_ * rhs.w_),
			(m20_ * rhs.x_ + m21_ * rhs.y_ + m22_ * rhs.z_ + m23_ * rhs.w_)
			);
	}

	/// Add a matrix.
	Matrix3x4 operator + (const Matrix3x4& rhs) const
	{
		return Matrix3x4(
			m00_ + rhs.m00_,
			m01_ + rhs.m01_,
			m02_ + rhs.m02_,
			m03_ + rhs.m03_,
			m10_ + rhs.m10_,
			m11_ + rhs.m11_,
			m12_ + rhs.m12_,
			m13_ + rhs.m13_,
			m20_ + rhs.m20_,
			m21_ + rhs.m21_,
			m22_ + rhs.m22_,
			m23_ + rhs.m23_
			);
	}

	/// Subtract a matrix.
	Matrix3x4 operator - (const Matrix3x4& rhs) const
	{
		return Matrix3x4(
			m00_ - rhs.m00_,
			m01_ - rhs.m01_,
			m02_ - rhs.m02_,
			m03_ - rhs.m03_,
			m10_ - rhs.m10_,
			m11_ - rhs.m11_,
			m12_ - rhs.m12_,
			m13_ - rhs.m13_,
			m20_ - rhs.m20_,
			m21_ - rhs.m21_,
			m22_ - rhs.m22_,
			m23_ - rhs.m23_
			);
	}

	/// Multiply with a scalar.
	Matrix3x4 operator * (float rhs) const
	{
		return Matrix3x4(
			m00_ * rhs,
			m01_ * rhs,
			m02_ * rhs,
			m03_ * rhs,
			m10_ * rhs,
			m11_ * rhs,
			m12_ * rhs,
			m13_ * rhs,
			m20_ * rhs,
			m21_ * rhs,
			m22_ * rhs,
			m23_ * rhs
			);
	}

	/// Multiply a matrix.
	Matrix3x4 operator * (const Matrix3x4& rhs) const
	{
		return Matrix3x4(
			m00_ * rhs.m00_ + m01_ * rhs.m10_ + m02_ * rhs.m20_,
			m00_ * rhs.m01_ + m01_ * rhs.m11_ + m02_ * rhs.m21_,
			m00_ * rhs.m02_ + m01_ * rhs.m12_ + m02_ * rhs.m22_,
			m00_ * rhs.m03_ + m01_ * rhs.m13_ + m02_ * rhs.m23_ + m03_,
			m10_ * rhs.m00_ + m11_ * rhs.m10_ + m12_ * rhs.m20_,
			m10_ * rhs.m01_ + m11_ * rhs.m11_ + m12_ * rhs.m21_,
			m10_ * rhs.m02_ + m11_ * rhs.m12_ + m12_ * rhs.m22_,
			m10_ * rhs.m03_ + m11_ * rhs.m13_ + m12_ * rhs.m23_ + m13_,
			m20_ * rhs.m00_ + m21_ * rhs.m10_ + m22_ * rhs.m20_,
			m20_ * rhs.m01_ + m21_ * rhs.m11_ + m22_ * rhs.m21_,
			m20_ * rhs.m02_ + m21_ * rhs.m12_ + m22_ * rhs.m22_,
			m20_ * rhs.m03_ + m21_ * rhs.m13_ + m22_ * rhs.m23_ + m23_
			);
	}

	/// Multiply a 4x4 matrix.
	Matrix4 operator * (const Matrix4& rhs) const
	{
		return Matrix4(
			m00_ * rhs.m00_ + m01_ * rhs.m10_ + m02_ * rhs.m20_ + m03_ * rhs.m30_,
			m00_ * rhs.m01_ + m01_ * rhs.m11_ + m02_ * rhs.m21_ + m03_ * rhs.m31_,
			m00_ * rhs.m02_ + m01_ * rhs.m12_ + m02_ * rhs.m22_ + m03_ * rhs.m32_,
			m00_ * rhs.m03_ + m01_ * rhs.m13_ + m02_ * rhs.m23_ + m03_ * rhs.m33_,
			m10_ * rhs.m00_ + m11_ * rhs.m10_ + m12_ * rhs.m20_ + m13_ * rhs.m30_,
			m10_ * rhs.m01_ + m11_ * rhs.m11_ + m12_ * rhs.m21_ + m13_ * rhs.m31_,
			m10_ * rhs.m02_ + m11_ * rhs.m12_ + m12_ * rhs.m22_ + m13_ * rhs.m32_,
			m10_ * rhs.m03_ + m11_ * rhs.m13_ + m12_ * rhs.m23_ + m13_ * rhs.m33_,
			m20_ * rhs.m00_ + m21_ * rhs.m10_ + m22_ * rhs.m20_ + m23_ * rhs.m30_,
			m20_ * rhs.m01_ + m21_ * rhs.m11_ + m22_ * rhs.m21_ + m23_ * rhs.m31_,
			m20_ * rhs.m02_ + m21_ * rhs.m12_ + m22_ * rhs.m22_ + m23_ * rhs.m32_,
			m20_ * rhs.m03_ + m21_ * rhs.m13_ + m22_ * rhs.m23_ + m23_ * rhs.m33_,
			rhs.m30_,
			rhs.m31_,
			rhs.m32_,
			rhs.m33_
			);
	}

	/// Set translation elements.
	void SetTranslation(const vec3f& translation)
	{
		m03_ = translation.x_;
		m13_ = translation.y_;
		m23_ = translation.z_;
	}

	/// Set rotation elements from a 3x3 matrix.
	void SetRotation(const Matrix3& rotation)
	{
		m00_ = rotation.m00_;
		m01_ = rotation.m01_;
		m02_ = rotation.m02_;
		m10_ = rotation.m10_;
		m11_ = rotation.m11_;
		m12_ = rotation.m12_;
		m20_ = rotation.m20_;
		m21_ = rotation.m21_;
		m22_ = rotation.m22_;
	}

	/// Set scaling elements.
	void SetScale(const vec3f& scale)
	{
		m00_ = scale.x_;
		m11_ = scale.y_;
		m22_ = scale.z_;
	}

	/// Set uniform scaling elements.
	void SetScale(float scale)
	{
		m00_ = scale;
		m11_ = scale;
		m22_ = scale;
	}

	/// Return the combined rotation and scaling matrix.
	Matrix3 ToMatrix3() const
	{
		return Matrix3(
			m00_,
			m01_,
			m02_,
			m10_,
			m11_,
			m12_,
			m20_,
			m21_,
			m22_
			);
	}

	/// Return the rotation matrix with scaling removed.
	Matrix3 RotationMatrix() const
	{
		vec3f invScale(
			1.0f / sqrtf(m00_ * m00_ + m10_ * m10_ + m20_ * m20_),
			1.0f / sqrtf(m01_ * m01_ + m11_ * m11_ + m21_ * m21_),
			1.0f / sqrtf(m02_ * m02_ + m12_ * m12_ + m22_ * m22_)
			);

		return ToMatrix3().Scaled(invScale);
	}

	/// Return the translation part.
	vec3f Translation() const
	{
		return vec3f(
			m03_,
			m13_,
			m23_
			);
	}

	/// Return the rotation part.
	Quaternion Rotation() const { return Quaternion(RotationMatrix()); }

	/// Return the scaling part.
	vec3f Scale() const
	{
		return vec3f(
			sqrtf(m00_ * m00_ + m10_ * m10_ + m20_ * m20_),
			sqrtf(m01_ * m01_ + m11_ * m11_ + m21_ * m21_),
			sqrtf(m02_ * m02_ + m12_ * m12_ + m22_ * m22_)
			);
	}

	/// Return decomposition to translation, rotation and scale.
	void Decompose(vec3f& translation, Quaternion& rotation, vec3f& scale) const;
	/// Return inverse.
	Matrix3x4 Inverse() const;

	/// Return float data.
	const float* Data() const { return &m00_; }

	float m00_;
	float m01_;
	float m02_;
	float m03_;
	float m10_;
	float m11_;
	float m12_;
	float m13_;
	float m20_;
	float m21_;
	float m22_;
	float m23_;

	/// Zero matrix.
	static const Matrix3x4 ZERO;
	/// Identity matrix.
	static const Matrix3x4 IDENTITY;
};

/// Multiply a 3x4 matrix with a scalar.
inline Matrix3x4 operator * (float lhs, const Matrix3x4& rhs) { return rhs * lhs; }

/// Multiply a 3x4 matrix with a 4x4 matrix.
inline Matrix4 operator * (const Matrix4& lhs, const Matrix3x4& rhs)
{
	return Matrix4(
		lhs.m00_ * rhs.m00_ + lhs.m01_ * rhs.m10_ + lhs.m02_ * rhs.m20_,
		lhs.m00_ * rhs.m01_ + lhs.m01_ * rhs.m11_ + lhs.m02_ * rhs.m21_,
		lhs.m00_ * rhs.m02_ + lhs.m01_ * rhs.m12_ + lhs.m02_ * rhs.m22_,
		lhs.m00_ * rhs.m03_ + lhs.m01_ * rhs.m13_ + lhs.m02_ * rhs.m23_ + lhs.m03_,
		lhs.m10_ * rhs.m00_ + lhs.m11_ * rhs.m10_ + lhs.m12_ * rhs.m20_,
		lhs.m10_ * rhs.m01_ + lhs.m11_ * rhs.m11_ + lhs.m12_ * rhs.m21_,
		lhs.m10_ * rhs.m02_ + lhs.m11_ * rhs.m12_ + lhs.m12_ * rhs.m22_,
		lhs.m10_ * rhs.m03_ + lhs.m11_ * rhs.m13_ + lhs.m12_ * rhs.m23_ + lhs.m13_,
		lhs.m20_ * rhs.m00_ + lhs.m21_ * rhs.m10_ + lhs.m22_ * rhs.m20_,
		lhs.m20_ * rhs.m01_ + lhs.m21_ * rhs.m11_ + lhs.m22_ * rhs.m21_,
		lhs.m20_ * rhs.m02_ + lhs.m21_ * rhs.m12_ + lhs.m22_ * rhs.m22_,
		lhs.m20_ * rhs.m03_ + lhs.m21_ * rhs.m13_ + lhs.m22_ * rhs.m23_ + lhs.m23_,
		lhs.m30_ * rhs.m00_ + lhs.m31_ * rhs.m10_ + lhs.m32_ * rhs.m20_,
		lhs.m30_ * rhs.m01_ + lhs.m31_ * rhs.m11_ + lhs.m32_ * rhs.m21_,
		lhs.m30_ * rhs.m02_ + lhs.m31_ * rhs.m12_ + lhs.m32_ * rhs.m22_,
		lhs.m30_ * rhs.m03_ + lhs.m31_ * rhs.m13_ + lhs.m32_ * rhs.m23_ + lhs.m33_
		);
}

}